state_angle.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_angle.h"
 
namespace wolf
{
class StateAngle : public StateBlock
{
  public:
    StateAngle(double _angle, bool _fixed = false, bool _transformable = true);
    StateAngle(const Eigen::VectorXd& _angle, bool _fixed = false, bool _transformable = true);
 
    static Eigen::VectorXd Identity();
 
    WOLF_STATE_BLOCK_CREATE(StateAngle);
 
    ~StateAngle() override;
 
    virtual void transform(const VectorComposite& _transformation) override;
};
 
inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable)
    : StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
    state_(0) = pi2pi(_angle);
}
 
inline StateAngle::StateAngle(const Eigen::VectorXd& _angle, bool _fixed, bool _transformable)
    : StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
    if (_angle.size() != 1) throw std::runtime_error("The angle state vector must be of size 1!");
 
    state_(0) = pi2pi(_angle(0));
}
 
inline Eigen::VectorXd StateAngle::Identity()
{
    return Eigen::VectorXd::Zero(1);
}
 
inline StateAngle::~StateAngle()
{
    //
}
 
inline void StateAngle::transform(const VectorComposite& _transformation)
{
    if (isTransformable()) setState(_transformation.at('O') + getState());  // 2D rotation is a sum of angles
}
 
} /* namespace wolf */